clear,clc,close all
format bank
%%
%初始化
%芯模尺寸参数
a=77.5;b=a/2;R=a;h=30;L=155;tc1=150;tc2=5;
%工件坐标系原点
x_tool=-358.5;z_tool=923;y_tool=1664;%xtool绝对值要加大
%轨迹姿态
% A=0;B=0;C_right=110;C_left=110;
A=90;B=0;C_right=95;C_left=95;
%工具长度、液压微调量
% Tool=262;Fa=5;
Tool=252;Fa=0;
%%
%螺旋铺放
%计算芯模铺放角、芯模旋转角
[ftpf,tspf,tsxz,ftxz] = pf_xz(a,b,R,h,L);
%生产铺放轨迹
[T1,T21,T22,T3,T_first] = pose(a,b,R,h,tc1,tc2,x_tool,z_tool,y_tool,A,B,C_right,C_left);
%逆解机器人关节角度
[IK1,IK21,IK22,IK3] = ik_kuka(T1,T21,T22,T3,Tool,Fa);
%梳理关节角度、给出安全点
Tu(T1,tc1,tc2,ftxz,tsxz,ftpf,tspf,IK1,IK21,IK22,IK3,T_first,Tool);
%生成dat、src库卡执行文件
dat_src;
%角度处理
load joint_speed.mat;
joint_cha=successive(joint);
save('joint_cha.mat',"joint_cha");
%生成连续铺放库卡执行文件
dat_src_p;
%验证轨迹准确度
load joint.mat;
POS = inspect(x_tool,y_tool,z_tool,R,Tool,joint);
max(POS(:,1))